#!/usr/bin/python
# -*- coding: UTF-8 -*-

from MiniBotControlLib import MiniBotControlLib
import time
# ------------------------------
# 	1、连接设备 Connect
#   2、扫描模块ID ScanId
#   3、设置关节模块模式 SetMotorMode
#   4、设置关节模块位置角度（位置模式） SetMotorPosition
#   5、设置关节位置角度及目标运行速度(位置模式) SetPositionWithSpeed
#   6、同步设置多个关节模块位置(位置模式) SetMotorPositionSync
#   7、同步设置多个关节模块位置带目标速度(位置模式) SetMotorPositionSyncWithSpeeds
#   8、 获取关节模块当前位置角度
# 	9、断开设备 Disconnect
# ------------------------------

# 声明控制库对象
mlib = MiniBotControlLib("udp")


# *************************
#   1、连接设备
#*************************#
ip = "10.10.100.254"
r = mlib.Connect(ip, 8899)
# 带超时的连接方式
#r = mlib.Connect(ip,8899,2000)
if(r > 0):
    print("连接成功")
else:
    print("连接失败")


# *************************
#   2、扫描模块ID
# *************************
motorIds, motorLocs = mlib.ScanId(0, 50)
print("motorIds", motorIds)
print("motorLocs", motorLocs)


# *************************
#   3、 获取关节模块当前位置角度
# *************************
input("输入任意字符回车， 获取关节模块当前位置角度")
for id in motorIds:
    pos = mlib.GetMotorPosition(id)
    print("模块ID："+str(id) + "  位置: "+str(pos))


# *************************
#   4、设置关节模块模式
# *************************
input("输入任意字符回车，设置模块为位置模式")
for id in motorIds:
    r = mlib.SetMotorMode(id, 0)
    if(r):
        print("模块ID："+str(id) + "设置成功")
    else:
        print("模块ID："+str(id) + "设置失败")
time.sleep(0.5)


# *************************
#   5、设置关节模块位置角度（位置模式）
# *************************
input("输入任意字符回车，设置关节模块位置角度 位置为90度")
for id in motorIds:
    r = mlib.SetMotorPosition(id, 90, 2)
    if(r):
        print("模块ID："+str(id) + "设置成功")
    else:
        print("模块ID："+str(id) + "设置失败")


# *************************
#  6、设置关节位置角度及目标运行速度(位置模式)
# *************************
input("输入任意字符回车，设置关节位置角度及目标运行速度 位置为-90度")
for id in motorIds:
    r = mlib.SetPositionWithSpeed(id, -90, 5, 2)
    if(r):
        print("模块ID："+str(id) + "设置成功")
    else:
        print("模块ID："+str(id) + "设置失败")


# *************************
#   7、同步设置多个关节模块位置(位置模式)
# *************************
input("输入任意字符回车，同步设置多个关节模块位置 位置为0度")
r = mlib.SetMotorPositionSync(motorIds, [0]*len(motorIds), 2)
if(r):
    print("设置成功")
else:
    print("设置失败")


# *************************
#   8、同步设置多个关节模块位置带目标速度(位置模式)
# *************************
input("输入任意字符回车，同步设置多个关节模块位置带目标速度 位置为90度")
r = mlib.SetMotorPositionSyncWithSpeeds(motorIds, [90]*len(motorIds), [20]*len(motorIds), 2)
if(r):
    print("设置成功")
else:
    print("设置失败")


# *************************
#   9、 获取关节模块当前位置角度
# *************************
input("输入任意字符回车， 获取关节模块当前位置角度")
for id in motorIds:
    pos = mlib.GetMotorPosition(id)
    print("模块ID："+str(id) + "位置"+str(pos))


# *************************
# 	10、断开设备
# *************************
input("输入任意字符回车，断开连接")
r = mlib.Disconnect()
if(r > 0):
    print("断开成功")
else:
    print("断开失败")
